#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
//#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <tf2_ros/transform_listener.h>
#include "ground_segmentation/ground_segmentation.h"
#include "pcl/filters/filter.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/crop_box.h>
#include "glog/logging.h"
#include "tools/file_manager.hpp"
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <nav_msgs/GridCells.h>
#include <set>

//VoxelGrid include
#include <nav_msgs/Odometry.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <std_msgs/UInt8MultiArray.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include "msgs/PCArray.h"
using namespace Eigen;

using namespace std;
string WORK_SPACE_PATH;
string camera_input_topic;

struct GridNode
{
    float GrassHigh;
    float ElevGrassHigh; //草高
    float minZaxisElev;//栅格中的Z轴最低点
    bool isObstacle;
    bool isGrass;
    bool isUnknown;
    bool enable; // todo enable 或许可以去掉

    GridNode() {
        GrassHigh = -100;
        ElevGrassHigh = -100;
        minZaxisElev = 100;
        isGrass = false;
        isObstacle = false;
        isUnknown = true;
        // todo 是不是可以改成访问该点了之后再分配内存
    };
    void clear(){
        GrassHigh = -100;
        ElevGrassHigh = -100;
        minZaxisElev = -100;
        isGrass = false;
        isObstacle = false;
        isUnknown = true;
    }
};
typedef GridNode* GridNodePtr;
GridNodePtr ** GridNodeMap;

// todo 点云不存在GridNode中，会占用内存
struct GridCloudNode{
    pcl::PointCloud<pcl::PointXYZ> Cloud;

    GridCloudNode(){}

    void ClearCloud(){
        Cloud.clear();
    }
};
typedef GridCloudNode* GridCloudNodePtr;
GridCloudNodePtr ** GridCloudNodeMap;

pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudCrop(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZI>::Ptr TempPointCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZ>::Ptr CameraDeepCloud(new pcl::PointCloud<pcl::PointXYZ>());

pcl::CropBox<pcl::PointXYZ> boundary_box_filter_;
pcl::CropBox<pcl::PointXYZ> nearby_box_filter_;

pcl::CropBox<pcl::PointXYZ> DeepCamera_bbox_filter_;

ros::Publisher PC_Array_obj_pub_;
ros::Publisher GridCell_pub_;
ros::Publisher ObjPointCloudPub_;
ros::Publisher pub_MarkerArray_;
ros::Publisher pub_UInt8MultiArray_;
ros::Publisher camera_cloud_pub_;
ros::Publisher full_cloud_pub_;
ros::Subscriber camera_sub_;



double dcx = 425.70074462890625;
double dcy = 234.9811248779297;
double dfx = 429.1876220703125;
double dfy = 429.1876220703125;
float kScaleFactor = 1000;
float DeepCameraHigh = 0.25;

float fx = 1264.252,fy = 1264.52,cx = 650.92,cy = 536.64;


float LidarHigh = 0.408;
float terrainVoxelCenX = 0;
float terrainVoxelCenY = 0;
float quantileZ = 0.1;

bool newlaserCloud = false;
bool newOdometry = false;
bool Odometry_init = false;

float minRelZ = -1.5;
float maxRelZ = 0.2;
float disRatioZ = 0.2;
float minmovedis = 1;
float minmoveyaw = 0.5;

float GridSize = 0.5;
int GridSizeInverse = 1.0/GridSize;
int GridWidth = 201;
int GridHalfWidth = (GridWidth - 1) / 2;
//float UpdateMapThrSqare = (GridSize*(GridWidth - 1)/4)*(GridSize*(GridWidth - 1)/4);//设为地图的1/4长度,但以平方表示
float UpdateMapThrSqare = (GridSize*(GridWidth - 1)/20)*(GridSize*(GridWidth - 1)/20);
const int GridCloudWidth = 81;
int GridCloudHalfWidth = (GridCloudWidth - 1) / 2;
float GridCloudMaxDisSquare = (GridSize * (GridCloudHalfWidth + 1))*(GridSize * (GridCloudHalfWidth + 1));
int minGridCloudNum = 20;
float minDiffElevGrassHigh = 0.1;
double laserCloudTime = 0;
float maxGrassHigh = 0.15;
double systemInitTime = 0;
bool systemInited = false;
bool newCameraCloud = false;

int noDataInited = 0;
int DiffWidth = GridHalfWidth - GridCloudHalfWidth;
const int GridNum = GridCloudWidth*GridCloudWidth;
int cluster_flag[GridNum] = {0}; // todo 这里需要修改一下，不然总是用const变量
int high_flag[GridNum] = {0}; // todo 这里需要修改一下，不然总是用const变量

float vehicleRoll = 0, vehiclePitch = 0, vehicleYaw = 0;
float vehicleX = 0, vehicleY = 0, vehicleZ = 0;
float PrevehicleRoll = 0, PrevehiclePitch = 0, PrevehicleYaw = 0;
float PrevehicleX = 0, PrevehicleY = 0, PrevehicleZ = 0;

//float vehicleXRec = 0, vehicleYRec = 0;

float sinVehicleRoll = 0, cosVehicleRoll = 0;
float sinVehiclePitch = 0, cosVehiclePitch = 0;
float sinVehicleYaw = 0, cosVehicleYaw = 0;

//lidar->camera
Eigen::Matrix3d lidar2camera_R;
Eigen::Vector3d lidar2camera_T;

//deepcamera ->camera
Eigen::Matrix3d dcamera2lidar_R;
Eigen::Vector3d dcamera2lidar_T;

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odom) {
    double roll, pitch, yaw;
     geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation;
    tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w))
            .getRPY(roll, pitch, yaw);

    vehicleRoll = roll;
    vehiclePitch = pitch;
    vehicleYaw = yaw;
    vehicleX = odom->pose.pose.position.x;
    vehicleY = odom->pose.pose.position.y;
    vehicleZ = odom->pose.pose.position.z;

    sinVehicleRoll = sin(vehicleRoll);
    cosVehicleRoll = cos(vehicleRoll);
    sinVehiclePitch = sin(vehiclePitch);
    cosVehiclePitch = cos(vehiclePitch);
    sinVehicleYaw = sin(vehicleYaw);
    cosVehicleYaw = cos(vehicleYaw);

    newOdometry = true;
    Odometry_init = true;

}

void LidarToMap(pcl::PointCloud<pcl::PointXYZ>::Ptr &laserCloud){
    int CloudSize = laserCloud->size();

    for(int i = 0;i<CloudSize;i++){

        // 先从雷达坐标系转换到imu坐标系
        float temp;
        temp = laserCloud->points[i].x;
        laserCloud->points[i].x = laserCloud->points[i].y;
        laserCloud->points[i].y = -temp;

        laserCloud->points[i].x = laserCloud->points[i].x + 0.1;
        laserCloud->points[i].z = laserCloud->points[i].z + 0.15;

        //从imu坐标系转换到全局坐标系
        float pointX1 = laserCloud->points[i].x;//获得雷达的局部坐标
        float pointY1 = laserCloud->points[i].y;
        float pointZ1 = laserCloud->points[i].z;

        float pointX2 =
                pointX1 * cosVehicleYaw - pointY1 * sinVehicleYaw;
        float pointY2 =
                + pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
        float pointZ2 = pointZ1;

        float pointX3 =
                pointX2 * cosVehiclePitch + pointZ2 * sinVehiclePitch;
        float pointY3 = pointY2;
        float pointZ3 =
               - pointX2 * sinVehiclePitch + pointZ2 * cosVehiclePitch;

        float pointX4 = pointX3;
        float pointY4 =
                pointY3 * cosVehicleRoll - pointZ3 * sinVehicleRoll;
        float pointZ4 =
                pointY3 * sinVehicleRoll + pointZ3 * cosVehicleRoll;

        pointX4 = pointX4 + vehicleX;//获得雷达的局部坐标
        pointY4 = pointY4 + vehicleY;
        pointZ4 = pointZ4 + vehicleZ;

        laserCloud->points[i].x = pointX4;
        laserCloud->points[i].y = pointY4;
        laserCloud->points[i].z = pointZ4;
    }

}

// registered laser scan callback function
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloud2) {

    laserCloudTime = laserCloud2->header.stamp.toSec();
    if (!systemInited) {
        systemInitTime = laserCloudTime;
        systemInited = true;
    }
//    if(Odometry_init == false)
//        return;

    laserCloud->clear();
    pcl::fromROSMsg(*laserCloud2, *laserCloud);
    ROS_INFO("the size of /zvision_lidar_points is %zu", laserCloud->points.size());
/*    std::vector<int> boxIndices_boundary;
    boundary_box_filter_.setInputCloud(laserCloud);
    boundary_box_filter_.filter(boxIndices_boundary);//
    boundary_box_filter_.filter(*laserCloud);

    std::vector<int> boxIndices_boundary2;
    nearby_box_filter_.setInputCloud(laserCloud);
    nearby_box_filter_.filter(boxIndices_boundary2);//
    nearby_box_filter_.filter(*laserCloud);

    //增加lidar 到imu,imu到世界坐标系的坐标变换
        LidarToMap();*/

    pcl::PointXYZ point;
    laserCloudCrop->clear();
    int laserCloudSize = laserCloud->points.size();

    for (int i = 0; i < laserCloudSize; i++) {
        point = laserCloud->points[i];

        float pointX = point.x;
        float pointY = point.y;
        float pointZ = point.z;
        //计算point相对于车的距离
        float dis = sqrt((pointX - vehicleX) * (pointX - vehicleX) +
                         (pointY - vehicleY) * (pointY - vehicleY));
        //可能小车在一个斜坡内，远处点可能比较高或者比较低，同时对点的距离进行限制
        if (pointZ - vehicleZ > minRelZ - disRatioZ * dis &&
            pointZ - vehicleZ < maxRelZ + disRatioZ * dis){
//            && dis < GridCloudMaxDisSquare) { // todo 获得的是特征点云，本来就挺少的，就不做距离限制了
            point.x = pointX;
            point.y = pointY;
            point.z = pointZ;
            laserCloudCrop->push_back(point);
        }
    }
    ROS_INFO("the size of laserCloudCrop is %zu", laserCloudCrop->points.size());

    std::vector<int> boxIndices_boundary;
    boundary_box_filter_.setInputCloud(laserCloudCrop);
    boundary_box_filter_.filter(boxIndices_boundary);//
    boundary_box_filter_.filter(*laserCloudCrop);

    std::vector<int> boxIndices_boundary2;
    nearby_box_filter_.setInputCloud(laserCloudCrop);
    nearby_box_filter_.filter(boxIndices_boundary2);//
    nearby_box_filter_.filter(*laserCloudCrop);

    newlaserCloud = true;

//    sensor_msgs::PointCloud2 laserCloudTemp;
//    if (ObjPointCloudPub_.getNumSubscribers() != 0) {
//        pcl::toROSMsg(*laserCloudCrop, laserCloudTemp);
//        laserCloudTemp.header.stamp = ros::Time::now();
//        laserCloudTemp.header.frame_id = "camera_init";
//        ObjPointCloudPub_.publish(laserCloudTemp);
//    }


}

void Deep2PointCloud(const sensor_msgs::ImageConstPtr & Depth_row_image) {

//    fullPointCloud->clear();
//    cloudHeader = Depth_row_image->header;TYPE_16UC1
    auto cv_ptr =  cv_bridge::toCvCopy(*Depth_row_image, sensor_msgs::image_encodings::TYPE_8UC3);
    cv::Mat Depth_img = cv_ptr->image;

    int rows = Depth_img.rows, cols = Depth_img.cols;
    CameraDeepCloud->clear();
    pcl::PointXYZ thisPoint;
    //recognize top rows cause no use
    for (int i = 50; i < rows; i+=1)
    {
        for (int j = 0; j < cols; j+=1)
        {
            thisPoint.y = (double)(Depth_img.at<unsigned short>(i, j)) / kScaleFactor;
            if (isnan(thisPoint.y) || thisPoint.y <= 0)
                continue;

            thisPoint.x = -((double)j - dcx) * thisPoint.y / dfx;

            thisPoint.z = -((double)i - dcy) * thisPoint.y / dfy;
            CameraDeepCloud->push_back(thisPoint);
        }
    }

/*    for (int i = 0; i < rows; i+=1)
    {
        for (int j = 0; j < cols; j+=1)
        {
            thisPoint.y = (double)(Depth_img.at<unsigned short>(i, j)) / kScaleFactor;
            if (isnan(thisPoint.y) || thisPoint.y <= 0 || thisPoint.y >= 5)
                continue;

            thisPoint.x = -((double)j - cx) * thisPoint.y / fx;
            if(thisPoint.x < -5 || thisPoint.x > 5)
                continue;

            thisPoint.z = -((double)i - cy) * thisPoint.y / fy;
            if(thisPoint.z > 2 || thisPoint.z < -DeepCameraHigh - 0.2)
                continue;
            CameraDeepCloud->push_back(thisPoint);
        }
    }*/
}

void CameraHandler(const sensor_msgs::ImageConstPtr & Depth_row_image) {

//    static float avg_ms = 0;
//    static int loop = 1;
//    std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
    // ROS_INFO("CameraHandler");
    Deep2PointCloud(Depth_row_image);

    std::vector<int> boxIndices_boundary;
    DeepCamera_bbox_filter_.setInputCloud(CameraDeepCloud);
    DeepCamera_bbox_filter_.filter(boxIndices_boundary);//
    DeepCamera_bbox_filter_.filter(*CameraDeepCloud);

    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(CameraDeepCloud);
    sor.setLeafSize(0.05f, 0.05f, 0.05f);
    sor.filter(*CameraDeepCloud);

    int cloudsize = CameraDeepCloud->size();
    Eigen::Vector3d v3d;
//        lidar2camera_T<<-0.00305,0.42377,-0.09468;

    //dcamera->lidar
    for(int i = 0 ;i <cloudsize;i++){
        v3d<<CameraDeepCloud->points[i].x,CameraDeepCloud->points[i].y,CameraDeepCloud->points[i].z;
        v3d = dcamera2lidar_R.transpose()*v3d + lidar2camera_T;
        CameraDeepCloud->points[i].x = v3d.x();
        CameraDeepCloud->points[i].y = v3d.y();
        CameraDeepCloud->points[i].z = v3d.z();
    }

    newCameraCloud = true;

    //2.统计滤波 todo 把这个加上看看去草地顶端的效果 DeepCamera可以试试这个方法
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
//    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor1;
//    sor1.setInputCloud(CameraDeepCloud);
//    sor1.setMeanK(10); //K近邻搜索点个数
//    sor1.setStddevMulThresh(1); //标准差倍数
//    sor1.setNegative(false); //保留未滤波点（内点）
//    sor1.filter(*CameraDeepCloud);  //保存滤波结果到cloud_filter

//    std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
//    std::chrono::duration<double, std::milli> fp_ms = end - start;
//    avg_ms += fp_ms.count();
//    std::cout<<"avg time consume:"<<avg_ms/loop<<endl;
//    loop++;

//    sensor_msgs::PointCloud2 laserCloudTemp;
//    if (camera_cloud_pub_.getNumSubscribers() != 0) {
//        pcl::toROSMsg(*CameraDeepCloud, laserCloudTemp);
//        laserCloudTemp.header.stamp = ros::Time::now();
//        laserCloudTemp.header.frame_id = "camera_init";
//        camera_cloud_pub_.publish(laserCloudTemp);
//    }



}

bool CheckOdometry(){

    //去掉根号
    float dis = sqrt((vehicleX - PrevehicleX) * (vehicleX - PrevehicleX) +
                       (vehicleY - PrevehicleY) * (vehicleY - PrevehicleY));

    if(dis < minmovedis)
        return false;

    float angle = abs(vehicleYaw - PrevehicleYaw);

    if(angle < minmoveyaw)
        return false;

    return true;
}

void UpdateGridMap(){
    //地图右下角是地图起点GridNodeMap[0][0]，Y向上，X向左


    float dis = (terrainVoxelCenX - vehicleX)*(terrainVoxelCenX - vehicleX)
            + (terrainVoxelCenY - vehicleY) * (terrainVoxelCenY - vehicleY);

    if(dis < UpdateMapThrSqare)
        return;

    std::cout<<"start updateGridMap"<<endl;
    std::cout<<"vehicleX"<<vehicleX<<endl;
    std::cout<<"vehicleY"<<vehicleY<<endl;
    ///全局地图X轴向上，Y轴向左，右下角0，0
    //将小车重新移动到全局栅格图的中心
    while(terrainVoxelCenX - vehicleX < -GridSize){
        for(int indY = 0;indY < GridWidth;indY++){
            // todo 指针操作改为一次移动多个，而不是一次移动一个
            GridNodePtr TempGridNodePtr = GridNodeMap[GridWidth - 1][indY];
            for(int indX = 0; indX < GridWidth -1 ; indX++){
                GridNodeMap[indX][indY] = GridNodeMap[indX+1][indY];
            }
            GridNodeMap[0][indY] = TempGridNodePtr;
            GridNodeMap[0][indY]->clear();
        }
        terrainVoxelCenX += GridSize;
    }
    while(terrainVoxelCenX - vehicleX > GridSize){
        for(int indY = 0;indY < GridWidth;indY++){
            // todo 指针操作改为一次移动多个，而不是一次移动一个
            GridNodePtr TempGridNodePtr = GridNodeMap[0][indY];
            for(int indX = GridWidth - 1; indX >= 1 ; indX--){
                GridNodeMap[indX][indY] = GridNodeMap[indX-1][indY];
            }
            GridNodeMap[GridWidth-1][indY] = TempGridNodePtr;
            GridNodeMap[GridWidth-1][indY]->clear();
        }
        terrainVoxelCenX -= GridSize;
    }
    while(terrainVoxelCenY - vehicleY < -GridSize){
        for(int indX = 0;indX < GridWidth - 1 ;indX++){
            // todo 指针操作改为一次移动多个，而不是一次移动一个
            GridNodePtr TempGridNodePtr = GridNodeMap[indX][0];
            for(int indY = 0; indY < GridWidth -1 ; indY++){
                GridNodeMap[indX][indY] = GridNodeMap[indX][indY+1];
            }
            GridNodeMap[indX][GridWidth-1] = TempGridNodePtr;
            GridNodeMap[indX][GridWidth-1]->clear();
        }
        terrainVoxelCenY += GridSize;
    }
    while(terrainVoxelCenY - vehicleY > GridSize){
        for(int indX = 0;indX < GridWidth - 1;indX++){
            // todo 指针操作改为一次移动多个，而不是一次移动一个
            GridNodePtr TempGridNodePtr = GridNodeMap[indX][GridWidth-1];
            for(int indY = GridWidth - 1; indY >= 1 ; indY--){
                GridNodeMap[indX][indY] = GridNodeMap[indX][indY-1];
            }
            GridNodeMap[indX][0] = TempGridNodePtr;
            GridNodeMap[indX][0]->clear();
        }
        terrainVoxelCenY -= GridSize;
    }

    std::cout<<"EndterrainVoxelCenX:"<<terrainVoxelCenX;
    std::cout<<"EndterrainVoxelCenY:"<<terrainVoxelCenY;

}

void ProjectCloudToGrid(){

    std::vector<std::pair<int,int>> GridIdx;

    int CloudSize = laserCloudCrop->size();
    int indX,indY;

    // todo debug 模式下这个操作要0.75ms,之后可以考虑用到那一部分，那一部分clear一下,但是这样又要执行多个判断
    for(int i = 0;i<GridCloudWidth;i++)
        for(int j = 0;j<GridCloudWidth;j++)
            GridCloudNodeMap[i][j]->ClearCloud();

    for(int i = 0;i<CloudSize;i++){
        indX = floor(GridSizeInverse*(laserCloudCrop->points[i].x - vehicleX)) +GridCloudHalfWidth;
        indY = floor(GridSizeInverse*(laserCloudCrop->points[i].y - vehicleY)) +GridCloudHalfWidth;
        if(indX < 0 || indX > GridCloudWidth - 1 || indY < 0 || indY > GridCloudWidth - 1)
            continue;
        GridCloudNodeMap[indX][indY]->Cloud.push_back(laserCloudCrop->points[i]);
    }
}

inline static bool compare(const pcl::PointXYZ& p1,const pcl::PointXYZ& p2){
    return p1.z > p2.z;
}

void ComputeGridFeature2() {

    // todo 更细节的历史信息计算
    // todo 对斜坡和树上树叶的考虑
    TempPointCloud->clear();

    for (int i = 0; i < GridNum; i++) {
        cluster_flag[i] = 0;
        high_flag[i] = 0;
    }
    // car's position in global GridCell
    int indvx = int(2*(vehicleX - terrainVoxelCenX)) + GridHalfWidth - GridCloudHalfWidth;
    int indvy = int(2*(vehicleY - terrainVoxelCenY)) + GridHalfWidth - GridCloudHalfWidth;

    // if use the following code,global gridcell is useless
    for (int indX = 0; indX < GridCloudWidth; indX++) {
        for (int indY = 0; indY < GridCloudWidth; indY++) {
            GridNodeMap[indvx + indX][indvy + indY]->clear();
//            GridNodeMap[indvx + indX][indvy + indY]->isObstacle = false;
//            GridNodeMap[indvx + indX][indvy + indY]->isUnknown = true;
        }
    }
//    for (int indX = -GridCloudHalfWidth-4; indX < GridCloudHalfWidth+4; indX++) {
//        for (int indY = -GridCloudHalfWidth-4; indY < GridCloudHalfWidth+4; indY++) {
//            if(indX >= GridCloudHalfWidth || indY >= GridCloudHalfWidth || indX <= -GridCloudHalfWidth ||indY <= 0){
//                if(indvx + indX>=0 && indvx + indX<GridWidth &&indvy + indY>=0 && indvy + indY<GridWidth){
//                    GridNodeMap[indvx + indX][indvy + indY]->isObstacle = false;
//                    GridNodeMap[indvx + indX][indvy + indY]->isUnknown = true;
//                }
//            }
//        }
//    }

    //提取上一帧障碍物栅格及其邻域栅格坐标
    set<pair<int,int>> ObjectInd;

    for (int indX = 0; indX < GridCloudWidth; indX++) {
        for (int indY = 0; indY < GridCloudWidth; indY++) {
            if(GridNodeMap[indvx + indX][indvy + indY]->isObstacle == true
            && GridCloudNodeMap[indX][indY]->Cloud.size()> minGridCloudNum){
                for(int dx = -1;dx <=1;dx++){
                    for(int dy = -1;dy<=1;dy++){
                        if(indX+dx<0||indX+dx>GridCloudWidth-1||indY+dy<0||indY+dy>GridCloudWidth-1)
                            continue;
                        ObjectInd.insert(pair(indX+dx,indY+dy));
                        GridNodeMap[indvx + indX][indvy + indY]->isObstacle = false;// todo 栅格状态改Unknown
                    }
                }
            }
            else if(GridNodeMap[indvx + indX][indvy + indY]->isUnknown == true
                 && GridCloudNodeMap[indX][indY]->Cloud.size() > minGridCloudNum){
                int CloudSize = GridCloudNodeMap[indX][indY]->Cloud.size();

                int quantileID = int(quantileZ * CloudSize);
                sort(GridCloudNodeMap[indX][indY]->Cloud.points.begin(),
                     GridCloudNodeMap[indX][indY]->Cloud.points.end(),
                     compare);

                if (GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z >
                    GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z + maxGrassHigh) {
                    high_flag[indX * GridCloudWidth + indY] = 1; //1表示这个点可能是障碍物
                    GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh =
                            GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z
                            - GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z;

                } else if(GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z <
                          GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z + maxGrassHigh){
                    //否则就是草地
                    high_flag[indX * GridCloudWidth + indY] = 2;//2表示这个点是草地
                    GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh = GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z
                                                                             - GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z;
                    GridNodeMap[indvx + indX][indvy + indY]->isGrass = true;
                    GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
                    GridNodeMap[indvx + indX][indvy + indY]->isObstacle = false;

                }
            }
        }
    }

    for (int indX = 0; indX < GridCloudWidth; indX++) {
        for (int indY = 0; indY < GridCloudWidth; indY++) {
            if(high_flag[indX * GridCloudWidth + indY] == 1) {
                if (GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh > maxGrassHigh + 0.2) {
                    cluster_flag[indX * GridCloudWidth + indY] = 1;
                    GridNodeMap[indvx + indX][indvy + indY]->isObstacle = true;
                    GridNodeMap[indvx + indX][indvy + indY]->isGrass = false;
                    GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
                    continue;
                }
                else{
                    int count = 0;// 暂时采用计数方式，后面可以考虑用周围八个点的平均值的方式
                    for(int dx = -1;dx <= 1;dx++){
                        for(int dy = -1;dy <=1;dy++){
                            if((dx == 0 && dy == 0) || indX+dx<0||indX+dx>GridCloudWidth-1||indY+dy<0||indY+dy>GridCloudWidth-1)
                                continue;
                            if(GridNodeMap[indvx + indX+dx][indvy + indY+dy]->isUnknown)
                                continue;
                            if(GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh
                               > GridNodeMap[indvx + indX+dx][indvy + indY+dy]->ElevGrassHigh)
                                count++;
                        }
                    }
                    if(count >7){
                        cluster_flag[indX * GridCloudWidth + indY] = 1;
                        GridNodeMap[indvx + indX][indvy + indY]->isObstacle = true;
                        GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
                    }
                }
            }
        }
    }


    for(auto it = ObjectInd.begin();it != ObjectInd.end();it++){
        int indX = it->first;
        int indY = it->second;
        int CloudSize = GridCloudNodeMap[indX][indY]->Cloud.size();
        if(CloudSize < minGridCloudNum)
            continue;
        int quantileID = int(quantileZ * CloudSize);

        sort(GridCloudNodeMap[indX][indY]->Cloud.points.begin(),
             GridCloudNodeMap[indX][indY]->Cloud.points.end(),
             compare);

        if (GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z >
            GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z + maxGrassHigh+0.2){
            //高度差大于maxGrassHigh+0.2
            cluster_flag[indX * GridCloudWidth + indY] = 1;
            GridNodeMap[indvx + indX][indvy + indY]->isObstacle = true;
            GridNodeMap[indvx + indX][indvy + indY]->isGrass = false;
            GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
            GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh =
                    GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z
                    - GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z;
            continue;
        }else if(GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z <
                 GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z + maxGrassHigh){
            //高度差小于maxGrassHigh
            GridNodeMap[indvx + indX][indvy + indY]->isGrass = true;
            GridNodeMap[indvx + indX][indvy + indY]->isObstacle = false;
            GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
            GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh =
                    GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z
                    - GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z;
        }else{
            //高度在maxGrassHigh到maxGrassHigh+0.2之间,则用邻域信息计算
            int count = 0;
            for(int dx = -1;dx <= 1;dx++){
                for(int dy = -1;dy <=1;dy++){
                    if((dx == 0 && dy == 0) || indX+dx<0||indX+dx>GridCloudWidth-1||indY+dy<0||indY+dy>GridCloudWidth-1)
                        continue;
                    if(GridNodeMap[indvx + indX+dx][indvy + indX+dy]->isUnknown)
                        continue;
                    if(GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh
                       > GridNodeMap[indvx + indX+dx][indvy + indX+dy]->ElevGrassHigh)
                        count++;
                }
            }
            if(count >7){
                cluster_flag[indX * GridCloudWidth + indY] = 1;
                GridNodeMap[indvx + indX][indvy + indY]->isObstacle = true;
                GridNodeMap[indvx + indX][indvy + indY]->isGrass = false;
                GridNodeMap[indvx + indX][indvy + indY]->isUnknown = false;
            }

            GridNodeMap[indvx + indX][indvy + indY]->ElevGrassHigh =
                    GridCloudNodeMap[indX][indY]->Cloud.points[quantileID].z
                    - GridCloudNodeMap[indX][indY]->Cloud.points[CloudSize - 1].z;
        }
    }

}

void PublishGridCells(){
    //地图右下角是地图起点GridNodeMap[0][0]，Y向上，X向左

    nav_msgs::GridCells cells;

    cells.header.frame_id = "camera_init";
    cells.cell_height= GridSize;
    cells.cell_width = GridSize;
//    int indvx = int(2*(vehicleX - terrainVoxelCenX)) + GridHalfWidth - GridCloudHalfWidth;
//    int indvy = int(2*(vehicleY - terrainVoxelCenY)) + GridHalfWidth - GridCloudHalfWidth;

    geometry_msgs::Point obstacle;
    for(int indX = 0;indX<GridWidth;indX++){
        for(int indY = 0;indY<GridWidth;indY++) {
            if(GridNodeMap[indX][indY]->isObstacle == 1){
                obstacle.x = terrainVoxelCenX -GridSize*(GridHalfWidth-indX);
                obstacle.y = terrainVoxelCenY -GridSize*(GridHalfWidth-indY);

                // todo 可以改为该栅格最高点的值
                obstacle.z = 0;
                cells.cells.push_back(obstacle);
            }
        }
    }

//    for(int indX = 0;indX<GridCloudWidth;indX++){
//        for(int indY = 0;indY<GridCloudWidth;indY++) {
//            if(GridNodeMap[indvx + indX][indvy + indY]->isObstacle == 1){
//                obstacle.x = terrainVoxelCenX -GridSize*(indvx+indX-GridHalfWidth);
//                obstacle.y = terrainVoxelCenY -GridSize*(indvy+indY-GridHalfWidth);
//
//                // todo 可以改为该栅格最高点的值
//                obstacle.z = 0;
//                cells.cells.push_back(obstacle);
//            }
//        }
//    }

//    cells.cells.push_back(obstacle);

    GridCell_pub_.publish(cells);
    ROS_INFO("the size of /gridCell is : %zu", cells.cells.size());
}

void ClusterAndPubObjectGrid(){

    //栅格BFS聚类
    int label = 2;
    pcl::PointXYZI thispoint;
    int aa = 1;
    for(int indX = 0 ; indX <GridCloudWidth;indX++) {
        for (int indY = 0; indY < GridCloudWidth; indY++) {
            if (cluster_flag[indX * GridCloudWidth + indY] == 1) {
                vector<pair<int, int>> neighbour;

                neighbour.push_back(pair(indX, indY));
                while (!neighbour.empty()) {

                    pair<int, int> thisgrid = neighbour.back();
                    neighbour.pop_back();
                    int indx = thisgrid.first;
                    int indy = thisgrid.second;

                    cluster_flag[indx * GridCloudWidth + indy] = label;

                    int CloudSize = GridCloudNodeMap[indx][indy]->Cloud.size();
                    for (int i = 0; i < CloudSize; i++) {
                        thispoint.x = GridCloudNodeMap[indx][indy]->Cloud.points[i].x;
                        thispoint.y = GridCloudNodeMap[indx][indy]->Cloud.points[i].y;
                        thispoint.z = GridCloudNodeMap[indx][indy]->Cloud.points[i].z;
                        thispoint.intensity = label;
                        TempPointCloud->push_back(thispoint);
                    }

                    for (int dx = -1; dx <= 1; dx++) {
                        for (int dy = -1; dy <= 1; dy++) {
                            if (cluster_flag[(indx+dx) * GridCloudWidth + (indy + dy)] != 1 || (dx == 0 && dy == 0) ||
                                    indx + dx < 0 || indx + dx > GridCloudWidth - 1 || indy + dy < 0 ||
                                    indy + dy > GridCloudWidth - 1)
                                continue;
                            neighbour.push_back(pair(indx + dx, indy + dy));
                        }
                    }
                }
                label++;
            }
        }
    }

    sensor_msgs::PointCloud2 laserCloudTemp;
    if (ObjPointCloudPub_.getNumSubscribers() != 0) {
        pcl::toROSMsg(*TempPointCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = ros::Time::now();
        laserCloudTemp.header.frame_id = "map";  //camera_init
        ObjPointCloudPub_.publish(laserCloudTemp);
        ROS_INFO("the size of /TempPointCloud is : %zu", TempPointCloud->points.size());
    }
}

template<typename T>
void float2u8Arry(T &out, float floatdata)
{
    uint8_t uarray[4];
    *(float *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=3; ++i){
        out.data.push_back(uarray[i]);
    }

}

template<typename T>
void int162u8Arry(T &out, int16_t floatdata)
{
    uint8_t uarray[2];
    *(int16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data.push_back(uarray[i]);
    }
}

template<typename T>
void uint162u8Arry(T &out, uint16_t floatdata)
{
    uint8_t uarray[2];
    *(uint16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data.push_back(uarray[i]);
    }
}

template<typename T>
void TotalLen2u8Arry(T &out, uint16_t floatdata)
{
    uint8_t uarray[2];
    *(uint16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data[i] = uarray[i];
    }
}

void PublishROSMsg(visualization_msgs::MarkerArray &marker_array){
    std_msgs::UInt8MultiArray out;
    pcl::PointCloud<pcl::PointXYZ> thisCloud;
    pcl::PointXYZ thisPoint;

    uint8_t obstacle_num = 0;
    uint162u8Arry(out, 0);//total_length
    out.data.push_back(0);//total_object_num

    int MarkSize = marker_array.markers.size();

    //extract 8 conner points of obstacle
    for(visualization_msgs::Marker &mark : marker_array.markers){
        float scalex = mark.scale.x/2;
        float scaley = mark.scale.y/2;
        float scalez = mark.scale.z/2;

        for(int signx = -1;signx <=1;signx += 2){
            for(int signy = -1;signy<=1;signy +=2){
                for(int signz = -1;signz<=1;signz += 2){
                    thisPoint.x = mark.pose.position.x + signx*scalex;
                    thisPoint.y = mark.pose.position.y + signy*scaley;
                    thisPoint.z = mark.pose.position.z + signz*scalez;
                    thisCloud.push_back(thisPoint);
                }
            }
        }
    }

    //convert from map->imu->lidar->camera->pixel
    int PointSize = thisCloud.size();
    for(int i =0;i<PointSize;i++){
        thisPoint = thisCloud.points[i];
        //map to imu
        float PX4 = thisPoint.x - vehicleX;
        float PY4 = thisPoint.y - vehicleY;
        float PZ4 = thisPoint.z - vehicleZ;

        float PX3 = PX4;
        float PY3 = cosVehicleRoll*PY4 + sinVehicleRoll*PZ4;
        float PZ3 = cosVehicleRoll*PZ4 + sinVehicleRoll*PY4;

        float PX2 = PX3*cosVehiclePitch - PZ3*sinVehiclePitch;
        float PY2 = PY3;
        float PZ2 = PZ3*sinVehiclePitch - PZ3*cosVehiclePitch;

        float PX1 = PX2*cosVehicleYaw + PY2*sinVehicleYaw;
        float PY1 = - PX2*sinVehicleYaw + PY2*cosVehicleYaw ;
        float PZ1 = PZ2;

        //imu->lidar
        PX1 = PX1 - 0.1;
        PZ1 = PZ1 - 0.15;
        float temp;
        temp = PY1;
        PY1 = PX1;
        PX1 = -temp;

        //lidar->camera
//        Eigen::Matrix3d lidar2camera_R;
//        lidar2camera_R<<0.9994,-0.006152,0.034226,-0.034151,0.011869,0.99935,-0.0065542,-0.99991,0.01165;
        Eigen::Vector3d v3d;
        v3d<<PX1,PY1,PZ1;
//        lidar2camera_T<<-0.00305,0.42377,-0.09468;
        v3d = lidar2camera_R.transpose()*v3d + (-lidar2camera_R.transpose()*lidar2camera_T);

        //normalize
        v3d.x() = v3d.x()/v3d.z();
        v3d.y() = v3d.y()/v3d.z();
        v3d.z() = 1;

        //distort

        //camera->pixel
        thisCloud.points[i].x = fx*v3d.x()+cx;
        thisCloud.points[i].y = fy*v3d.y()+cy;
        thisCloud.points[i].z = 1;
    }

    int pointsize = thisCloud.size();

    for(int i = 0 ;i<pointsize;i += 8){
        uint162u8Arry(out, 21);//Length class:1,conficence:4,outline:4*2*2
        int datastart = out.data.size()-2;
        out.data.push_back(0);//class
        float2u8Arry(out, 0);//confidence

        int16_t minx,miny,maxx,maxy;
        minx = maxx = thisCloud.points[i].x;
        miny = maxy = thisCloud.points[i].y;
        for(int j = i+1;j<i+4;j++){
            if(thisCloud.points[j].x>maxx)maxx=thisCloud.points[j].x;
            if(thisCloud.points[j].x<minx)minx=thisCloud.points[j].x;
            if(thisCloud.points[j].y>maxy)maxy=thisCloud.points[j].y;
            if(thisCloud.points[j].y<miny)miny=thisCloud.points[j].y;
        }

        int162u8Arry(out,minx);//outline
        int162u8Arry(out,miny);
        int162u8Arry(out,maxx);
        int162u8Arry(out,maxy);

        minx = maxx = thisCloud.points[i+4].x;
        miny = maxy = thisCloud.points[i+4].y;
        for(int j = i+5;j<i+8;j++){
            if(thisCloud.points[j].x>maxx)maxx=thisCloud.points[j].x;
            if(thisCloud.points[j].x<minx)minx=thisCloud.points[j].x;
            if(thisCloud.points[j].y>maxy)maxy=thisCloud.points[j].y;
            if(thisCloud.points[j].y<miny)miny=thisCloud.points[j].y;
        }

        int162u8Arry(out,minx);//outline
        int162u8Arry(out,miny);
        int162u8Arry(out,maxx);
        int162u8Arry(out,maxy);
        obstacle_num++;
        out.data[datastart] = out.data.size() - (datastart+2);//set length of obstacle
    }

    TotalLen2u8Arry(out,out.data.size()-2);
    out.data[2] = obstacle_num;
    pub_UInt8MultiArray_.publish(out);
    ROS_INFO("the size of /cloud_point_ai is :%zu", out.data.size() );


}

void PubBoundingBox(){
    if(TempPointCloud->empty())
        return;

    visualization_msgs::Marker bbox_marker;
    bbox_marker.header.frame_id = "map";  //camera_init
    bbox_marker.ns = "";
    bbox_marker.color.r = 0.0f;
    bbox_marker.color.g = 1.0f;
    bbox_marker.color.b = 0.0f;
    bbox_marker.color.a = 0.5;
    bbox_marker.lifetime = ros::Duration();
    bbox_marker.frame_locked = true;
    bbox_marker.type = visualization_msgs::Marker::CUBE;
    bbox_marker.action = visualization_msgs::Marker::ADD;

    visualization_msgs::MarkerArray marker_array;
    msgs::PCArray PCArray_obj;
    pcl::PointCloud<pcl::PointXYZ> TempCloud;
    pcl::PointXYZ SumPoint,maxpoint,minpoint,temppoint;
    maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
    minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
    size_t CntNum = 0;
    size_t marker_id = 0;
    pcl::PointXYZ min;	//xyz的最小值
    pcl::PointXYZ max;	//xyz的最大值

    static size_t max_marker_size_ = 0;
    int flag_intensity = TempPointCloud->begin()->intensity;

//    int SIzeCloud = TempPointCloud->size();
    size_t cntcloudsize = 0;
    ROS_INFO("the size of TempPointCloud is : %zu",TempPointCloud->size());
    for(auto iter = TempPointCloud->begin();iter != TempPointCloud->end();iter++){
        if(iter->intensity == flag_intensity){
            SumPoint.x += iter->x;
            SumPoint.y += iter->y;
            SumPoint.z += iter->z;

            temppoint.x = iter->x;
            temppoint.y = iter->y;
            temppoint.z = iter->z;
            TempCloud.push_back(temppoint);

            CntNum++;
            cntcloudsize++;
            continue;
        }

        pcl::getMinMax3D(TempCloud,min,max);	//在聚类的坐标系中计算最大最小值

        bbox_marker.id = marker_id;
        bbox_marker.color.a = 0.5;
        bbox_marker.pose.position.x = SumPoint.x/CntNum;
        bbox_marker.pose.position.y = SumPoint.y/CntNum;
        bbox_marker.pose.position.z = SumPoint.z/CntNum;
        bbox_marker.scale.x = max.x - min.x;
        bbox_marker.scale.y = max.y - min.y;
        bbox_marker.scale.z = max.z - min.z;
        bbox_marker.header.stamp = ros::Time::now();
        if( bbox_marker.pose.position.y > 0){marker_array.markers.push_back(bbox_marker);}

        flag_intensity = iter->intensity;
        iter--;
        marker_id++;
        CntNum = 0;
        maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
        minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
        SumPoint.x = 0;SumPoint.y = 0;SumPoint.z = 0;

        sensor_msgs::PointCloud2 laserCloudTemp_PC_1;
        pcl::toROSMsg(TempCloud, laserCloudTemp_PC_1);
        laserCloudTemp_PC_1.header.stamp = ros::Time::now();
        laserCloudTemp_PC_1.header.frame_id = "map";  //camera_init
        if( bbox_marker.pose.position.y > 0){PCArray_obj.PC_Array.push_back(laserCloudTemp_PC_1);}

        TempCloud.clear();
    }

    pcl::getMinMax3D(TempCloud,min,max);	//在聚类的坐标系中计算最大最小值
    bbox_marker.id = marker_id;
    bbox_marker.color.a = 0.5;
    bbox_marker.pose.position.x = SumPoint.x/CntNum;
    bbox_marker.pose.position.y = SumPoint.y/CntNum;
    bbox_marker.pose.position.z = SumPoint.z/CntNum;
    bbox_marker.scale.x = max.x - min.x;
    bbox_marker.scale.y = max.y - min.y;
    bbox_marker.scale.z = max.z - min.z;
    bbox_marker.header.stamp = ros::Time::now();

    if( bbox_marker.pose.position.y > 0){marker_array.markers.push_back(bbox_marker);}

    sensor_msgs::PointCloud2 laserCloudTemp_PC_2;
    pcl::toROSMsg(TempCloud, laserCloudTemp_PC_2);
    laserCloudTemp_PC_2.header.stamp = ros::Time::now();
    laserCloudTemp_PC_2.header.frame_id = "map";  //camera_init
    if( bbox_marker.pose.position.y > 0){PCArray_obj.PC_Array.push_back(laserCloudTemp_PC_2);}


    PublishROSMsg(marker_array);

    if (marker_array.markers.size() > max_marker_size_){
        max_marker_size_ = marker_array.markers.size();

    }
    else{
        for (size_t i = marker_id+1; i < max_marker_size_; ++i)
        {
            bbox_marker.id = i;
            bbox_marker.color.a = 0;
            bbox_marker.pose.position.x = 0;
            bbox_marker.pose.position.y = 0;
            bbox_marker.pose.position.z = 0;
            bbox_marker.scale.x = 0;
            bbox_marker.scale.y = 0;
            bbox_marker.scale.z = 0;
            marker_array.markers.push_back(bbox_marker);
            ++marker_id;
        }
    }
//    ROS_INFO("the raw size of /LidarObjBox is : %zu", marker_array.markers.size());
//    for(int i =0;i < marker_array.markers.size();i++){
//        if(marker_array.markers[i].pose.position.y <= 0){
//            marker_array.markers.erase(marker_array.markers.begin()+i);
//            PCArray_obj.PC_Array.erase(PCArray_obj.PC_Array.begin()+i);
//            i--;
//        }
//    }
    ROS_INFO("******the now size of /LidarObjBox is : %zu", marker_array.markers.size());
    PC_Array_obj_pub_.publish(PCArray_obj);
    pub_MarkerArray_.publish(marker_array);



}

int main(int argc, char **argv) {
    ros::init(argc, argv, "ObjectSegVoxelGrid");

    ros::NodeHandle nh("~");

    // Do parameter stuff.
    {
        lidar2camera_R << 0.9994, -0.006152, 0.034226, -0.034151, 0.011869, 0.99935, -0.0065542, -0.99991, 0.01165;
        lidar2camera_T << -0.00305, 0.42377, -0.09468;
        vector<double> l2c_R, l2c_T;
        nh.param<vector<double>>("lidar2camera_T", l2c_T, l2c_T);
        nh.param<vector<double>>("lidar2camera_R", l2c_R, l2c_R);

        if (!l2c_R.empty()) {
            lidar2camera_R(0, 0) = l2c_R[0];
            lidar2camera_R(0, 1) = l2c_R[1];
            lidar2camera_R(0, 2) = l2c_R[2];
            lidar2camera_R(1, 0) = l2c_R[3];
            lidar2camera_R(1, 1) = l2c_R[4];
            lidar2camera_R(1, 2) = l2c_R[5];
            lidar2camera_R(2, 0) = l2c_R[6];
            lidar2camera_R(2, 1) = l2c_R[7];
            lidar2camera_R(2, 2) = l2c_R[8];
        }

        if (!l2c_T.empty()) {
            lidar2camera_T(0) = l2c_T[0];
            lidar2camera_T(1) = l2c_T[1];
            lidar2camera_T(2) = l2c_T[2];
        }

        dcamera2lidar_R << 1, 0, 0, 0, -1, 0, 0, 0, 1;
        dcamera2lidar_T << 0, 0, 0;
        vector<double> l2dc_R, l2dc_T;
        nh.param<vector<double>>("dcamera2lidar_T", l2dc_T, l2dc_T);
        nh.param<vector<double>>("dcamera2lidar_R", l2dc_R, l2dc_R);
        if (!l2dc_R.empty()) {
            dcamera2lidar_R(0, 0) = l2dc_R[0];
            dcamera2lidar_R(0, 1) = l2dc_R[1];
            dcamera2lidar_R(0, 2) = l2dc_R[2];
            dcamera2lidar_R(1, 0) = l2dc_R[3];
            dcamera2lidar_R(1, 1) = l2dc_R[4];
            dcamera2lidar_R(1, 2) = l2dc_R[5];
            dcamera2lidar_R(2, 0) = l2dc_R[6];
            dcamera2lidar_R(2, 1) = l2dc_R[7];
            dcamera2lidar_R(2, 2) = l2dc_R[8];
        }
        if (!l2dc_T.empty()) {
            dcamera2lidar_T(0) = l2dc_T[0];
            dcamera2lidar_T(1) = l2dc_T[1];
            dcamera2lidar_T(2) = l2dc_T[2];
        }


        nh.param<float>("minRelZ", minRelZ);
        nh.param<float>("maxRelZ", maxRelZ);
        nh.param<float>("disRatioZ", disRatioZ);
        nh.param<float>("VoxelGridSize", GridSize);
        nh.param<int>("GridWidth", GridWidth);
        nh.param<int>("GridCloudWidth", GridCloudWidth);

        nh.param<float>("minmovedis", minmovedis);
        nh.param<float>("minmoveyaw", minmoveyaw);
        nh.param<float>("UpdateMapThrSqare", UpdateMapThrSqare);
        nh.param<float>("LidarHigh", LidarHigh);
        nh.param<float>("minGridCloudNum", minGridCloudNum);
        nh.param<float>("minDiffElevGrassHigh", minDiffElevGrassHigh);
        nh.param<float>("maxGrassHigh", maxGrassHigh);

        nh.param<string>("WORK_SPACE_PATH", WORK_SPACE_PATH,
                         "/home/houj/FJY_0106/Obstacle-Cluster-master-5b1784ee8477661ee8bfb9470cce0826fd3d23b0/src/Object_Cluster_voxelGrid");
        nh.param<string>("camera_input_topic", camera_input_topic, "/hikrobot_camera/rgb");  // /camera/depth/image_rect_raw
        nh.param<double>("dcx", dcx);
        nh.param<double>("dcy", dcy);
        nh.param<double>("dfx", dfx);
        nh.param<double>("dfy", dfy);
        nh.param<double>("kScaleFactor", kScaleFactor);
        nh.param<float>("DeepCameraHigh", DeepCameraHigh);
        nh.param<float>("cx", cx);
        nh.param<float>("cy", cy);
        nh.param<float>("fx", fx);
        nh.param<float>("fy", fy);

        boundary_box_filter_.setMin(Eigen::Vector4f(-1.0, -0.0, -LidarHigh - 0.2, 1.0));
        boundary_box_filter_.setMax(Eigen::Vector4f(1.0, 20, 2, 1.0));
        boundary_box_filter_.setNegative(false);//保留框内的

        DeepCamera_bbox_filter_.setMin(Eigen::Vector4f(-5, -5, -DeepCameraHigh - 0.1, 1.0));
        DeepCamera_bbox_filter_.setMax(Eigen::Vector4f(5, 5, 1.5, 1.0));
        DeepCamera_bbox_filter_.setNegative(false);//保留框内的

        //zvision滤去没有接收到的点，没有接收到的点都是0，0，0，
        nearby_box_filter_.setMin(Eigen::Vector4f(-0.5, -0.0, -0.5, 1.0));
        nearby_box_filter_.setMax(Eigen::Vector4f(0.5, 0.5, 0.5, 1.0));
        nearby_box_filter_.setNegative(true);//保留框外的

        google::InitGoogleLogging(argv[0]);
        FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
        FLAGS_alsologtostderr = true;
        FLAGS_colorlogtostderr = true;
        FLAGS_log_prefix = true;
        FLAGS_logbufsecs = 0;
        FileManager::CreateDirectory(FLAGS_log_dir);

        GridHalfWidth = (GridWidth - 1) / 2;
        GridCloudHalfWidth = (GridCloudWidth - 1) / 2;
        GridCloudMaxDisSquare = (GridSize * (GridCloudHalfWidth + 1)) * (GridSize * (GridCloudHalfWidth + 1));
        GridSizeInverse = 1 / GridSize;
        DiffWidth = GridHalfWidth - GridCloudHalfWidth;
    }

    ros::Subscriber subOdometry =
            nh.subscribe<nav_msgs::Odometry>("/Odometry", 5, odometryHandler);

    // todo 直接用原始点云和特征点云之间的效果记得对比一下 /zvision_lidar_points
//    ros::Subscriber subLaserCloud =
//            nh.subscribe<sensor_msgs::PointCloud2>("/cloud_registered", 5, laserCloudHandler);
    ros::Subscriber subLaserCloud =
            nh.subscribe<sensor_msgs::PointCloud2>("/zvision_lidar_points", 5, laserCloudHandler);

    GridCell_pub_ = nh.advertise<nav_msgs::GridCells>("/gridCell", 1);
    ObjPointCloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("/TempPointCloud", 1);
    pub_MarkerArray_ = nh.advertise<visualization_msgs::MarkerArray>("/LidarObjBox",1);
    pub_UInt8MultiArray_ = nh.advertise<std_msgs::UInt8MultiArray>("/cloud_point_ai",5);
    camera_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/CameraPointCloud", 1);
    full_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/fullPointCloud", 1);
    PC_Array_obj_pub_ = nh.advertise<msgs::PCArray>("/PC_Array_obj", 1);

    camera_sub_ = nh.subscribe(camera_input_topic, 1, CameraHandler);

    GridNodeMap = new GridNodePtr * [GridWidth];
    for(int i = 0;i<GridWidth;i++){
        GridNodeMap[i] = new GridNodePtr  [GridWidth];
        for(int j = 0;j<GridWidth;j++){
            GridNodeMap[i][j]= new GridNode();
        }
    }

    GridCloudNodeMap = new GridCloudNodePtr * [GridCloudWidth];
    for(int i = 0;i<GridCloudWidth;i++){
        GridCloudNodeMap[i] = new GridCloudNodePtr  [GridCloudWidth];
        for(int j = 0;j<GridCloudWidth;j++){
            GridCloudNodeMap[i][j]= new GridCloudNode();
        }
    }


    ros::Rate rate(100);
    bool status = ros::ok();
    while (status) {

        ros::spinOnce();

        static float avg_ms = 0;
        static int loop = 1;
        std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();

        if(newCameraCloud && newlaserCloud){
            newCameraCloud = false;
            newlaserCloud = false;
        }
        else{
            continue;
        }
        *laserCloudCrop += *CameraDeepCloud;

        sensor_msgs::PointCloud2 laserCloudTemp;
        if (full_cloud_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*laserCloudCrop, laserCloudTemp);
            laserCloudTemp.header.stamp = ros::Time::now();
            laserCloudTemp.header.frame_id = "map";  //camera_init
            full_cloud_pub_.publish(laserCloudTemp);
            ROS_INFO("the size of full_cloud_pub_ is : %zu",laserCloudCrop->points.size());
        }

        UpdateGridMap();

        ProjectCloudToGrid();

        ComputeGridFeature2();

        ClusterAndPubObjectGrid();

        std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> fp_ms = end - start;
        avg_ms += fp_ms.count();
        std::cout<<"avg time consume:"<<avg_ms/loop<<endl;
        loop++;

        PubBoundingBox();

        PublishGridCells();


    }
}


